/**
 * @file /src/qnode.cpp
 *
 * @brief Ros communication central!
 *
 * @date February 2011
 **/

/*****************************************************************************
** Includes
*****************************************************************************/

#include <ros/ros.h>
#include <ros/network.h>
#include <string>
#include <std_msgs/String.h>


#include <sstream>
#include "../include/ui_core/qnode.hpp"

#include "QDebug"

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace ui_core {

/*****************************************************************************
** Implementation
*****************************************************************************/

QNode::QNode(int argc, char** argv ) :
	init_argc(argc),
	init_argv(argv)
{

}

QNode::~QNode() {
    if(ros::isStarted()) {
      ros::shutdown(); // explicitly needed since we use ros::start();
      ros::waitForShutdown();
    }
	wait();
}
bool QNode::init() {
	ros::init(init_argc,init_argv,"ui_core");
	if ( ! ros::master::check() ) {
		return false;
	}
	ros::start(); // explicitly needed since our nodehandle is going out of scope.
	ros::NodeHandle n;
	// Add your ros communications here.
	chatter_publisher = n.advertise<std_msgs::String>("chatter", 1000);
    rectangle_publisher = n.advertise<std_msgs::Int32MultiArray>("/draw_rectangle_topic", 1);

    tarcking_rectangle_sub = n.subscribe("track_rect_pub",100,&QNode::tracking_callback,this);

	start();

	return true;
}

bool QNode::init(const std::string &master_url, const std::string &host_url) {
	std::map<std::string,std::string> remappings;
	remappings["__master"] = master_url;
	remappings["__hostname"] = host_url;
	ros::init(remappings,"ui_core");
	if ( ! ros::master::check() ) {
		return false;
	}
	ros::start(); // explicitly needed since our nodehandle is going out of scope.
	ros::NodeHandle n;
	// Add your ros communications here.
	chatter_publisher = n.advertise<std_msgs::String>("chatter", 1000);

    rectangle_publisher = n.advertise<std_msgs::Int32MultiArray>("/rectangle_topic", 1);

    tarcking_rectangle_sub = n.subscribe("track_rect_pub",100,&QNode::tracking_callback,this);
	start();
	return true;
}

void QNode::sub_image(QString topic_name)
{
    ros::NodeHandle n;//ros节点初始化
    image_transport::ImageTransport it_(n);//创建图像句柄
    image_sub=it_.subscribe(topic_name.toStdString(),1000,&QNode::image_callback,this);//订阅图像话题，订阅到后执行image_callback操作(回调函数)

}

void QNode::tracking_callback(const std_msgs::Int32MultiArrayConstPtr &msg)
{
//    qDebug() << "x1:" <<  msg->data[0] << ",y1:" << msg->data[1] << ",x2:" << msg->data[2] << ",y2:" << msg->data[3];
    Q_EMIT tracking_val(msg->data[0], msg->data[1], msg->data[2], msg->data[3]);
}

void QNode::image_callback(const sensor_msgs::ImageConstPtr &msg)
{
    cv_bridge::CvImagePtr cv_ptr;//创建cv对象用来转换数据
    cv_ptr=cv_bridge::toCvCopy(msg,msg->encoding);
    QImage im=Mat2QImage(cv_ptr->image);
    Q_EMIT image_val(im);//发送转换完的数据作为信号
}
QImage QNode::Mat2QImage(cv::Mat const& src)	//转换图像数据的函数
{
  QImage dest(src.cols, src.rows, QImage::Format_RGB32);

  const float scale = 255.0;

  if (src.depth() == CV_8U)
  {
    if (src.channels() == 1) {
      for (int i = 0; i < src.rows; ++i) {
        for (int j = 0; j < src.cols; ++j) {
          int level = src.at<quint8>(i, j);
          dest.setPixel(j, i, qRgb(level, level, level));
        }
      }
    } else if (src.channels() == 3) {
      for (int i = 0; i < src.rows; ++i) {
        for (int j = 0; j < src.cols; ++j) {
          cv::Vec3b bgr = src.at<cv::Vec3b>(i, j);
          dest.setPixel(j, i, qRgb(bgr[0], bgr[1], bgr[2]));
        }
      }
    }
  }
  else if (src.depth() == CV_32F)
  {
    if (src.channels() == 1)
    {
      for (int i = 0; i < src.rows; ++i) {
        for (int j = 0; j < src.cols; ++j) {
          int level = scale * src.at<float>(i, j);
          dest.setPixel(j, i, qRgb(level, level, level));
        }
      }
    }
    else if (src.channels() == 3)
    {
      for (int i = 0; i < src.rows; ++i) {
        for (int j = 0; j < src.cols; ++j) {
          cv::Vec3f bgr = scale * src.at<cv::Vec3f>(i, j);
          dest.setPixel(j, i, qRgb(bgr[0], bgr[1], bgr[2]));
        }
      }
    }
  }

  return dest;
}

void QNode::send_rectangle_topic(std_msgs::Int32MultiArray int32M_Tmp)
{
    if(ros::master::check())
    {
        rectangle_publisher.publish(int32M_Tmp);
        log(Info,std::string("Select rectangle pos(x1,y1,x2,y2):") + std::to_string(int32M_Tmp.data[0]) + "," + std::to_string(int32M_Tmp.data[1]) + "," + std::to_string(int32M_Tmp.data[2]) + "," + std::to_string(int32M_Tmp.data[3]));
        log(Info,std::string("Track Starting!!!"));
    }
}


void QNode::run() {

    ros::spin();
	ros::Rate loop_rate(1);
	int count = 0;
	while ( ros::ok() ) {

		std_msgs::String msg;
		std::stringstream ss;
		ss << "hello world " << count;
		msg.data = ss.str();
		chatter_publisher.publish(msg);
		log(Info,std::string("I sent: ")+msg.data);
		ros::spinOnce();
		loop_rate.sleep();
		++count;
	}
	std::cout << "Ros shutdown, proceeding to close the gui." << std::endl;
	Q_EMIT rosShutdown(); // used to signal the gui for a shutdown (useful to roslaunch)
}


void QNode::log( const LogLevel &level, const std::string &msg) {
	logging_model.insertRows(logging_model.rowCount(),1);
	std::stringstream logging_model_msg;
	switch ( level ) {
		case(Debug) : {
				ROS_DEBUG_STREAM(msg);
				logging_model_msg << "[DEBUG] [" << ros::Time::now() << "]: " << msg;
				break;
		}
		case(Info) : {
				ROS_INFO_STREAM(msg);
				logging_model_msg << "[INFO] [" << ros::Time::now() << "]: " << msg;
				break;
		}
		case(Warn) : {
				ROS_WARN_STREAM(msg);
				logging_model_msg << "[INFO] [" << ros::Time::now() << "]: " << msg;
				break;
		}
		case(Error) : {
				ROS_ERROR_STREAM(msg);
                logging_model_msg << "[ERROR] [" << ros::Time::now() << "]: " << msg;
				break;
		}
		case(Fatal) : {
				ROS_FATAL_STREAM(msg);
				logging_model_msg << "[FATAL] [" << ros::Time::now() << "]: " << msg;
				break;
		}
	}
	QVariant new_row(QString(logging_model_msg.str().c_str()));
	logging_model.setData(logging_model.index(logging_model.rowCount()-1),new_row);
	Q_EMIT loggingUpdated(); // used to readjust the scrollbar
}

}  // namespace ui_core
